#include "Data.h"


Data::Data()
{
}

Data::~Data()
{
}

Data::Data(std::string FilePath, cv::Mat depth1, cv::Mat depth2, cv::Mat depth3, cv::Mat depth4,
	cv::Mat rgb1, cv::Mat rgb2, cv::Mat rgb3, cv::Mat rgb4)
{
	_FilePath = FilePath;
	_depth1 = depth1;
	_depth2 = depth2;
	_depth3 = depth3;
	_depth4 = depth4;
	_rgb1 = rgb1;
	_rgb2 = rgb2;
	_rgb3 = rgb3;
	_rgb4 = rgb4;
	this->ReadFile();

}

void Data::Update(cv::Mat depth1, cv::Mat depth2, cv::Mat depth3, cv::Mat depth4,
	cv::Mat rgb1, cv::Mat rgb2, cv::Mat rgb3, cv::Mat rgb4)
{
	this->Clear();
	_depth1 = depth1;
	_depth2 = depth2;
	_depth3 = depth3;
	_depth4 = depth4;
	_rgb1 = rgb1;
	_rgb2 = rgb2;
	_rgb3 = rgb3;
	_rgb4 = rgb4;
}

std::string Data::GetFilePath()
{
	return _FilePath;
}

std::vector<Eigen::Vector3d> Data::Get3dP()
{
	std::vector<Eigen::Vector3d> vP;
	vP.push_back(p1);
	vP.push_back(p2);
	vP.push_back(p3);
	vP.push_back(p4);
	return vP;
}
/*vP.push_back(p5);
	vP.push_back(p6);
	vP.push_back(p7);
	vP.push_back(p8);*/

std::vector<cv::Mat> Data::GetDepth()
{
	std::vector<cv::Mat> Vdimg_;
	Vdimg_.push_back(_depth1);
	Vdimg_.push_back(_depth2);
	Vdimg_.push_back(_depth3);
	Vdimg_.push_back(_depth4);
	return Vdimg_;
}

std::vector<Eigen::Matrix3d> Data::GetK()
{
	std::vector<Eigen::Matrix3d> K;
	K.push_back(_k1);
	K.push_back(_k2);
	K.push_back(_k3);
	K.push_back(_k4);
	return K;
}

std::vector<Eigen::Matrix3d> Data::GetR()
{
	std::vector<Eigen::Matrix3d> R;
	R.push_back(_T1.block(0, 0, 3, 3));
	R.push_back(_T2.block(0, 0, 3, 3));
	R.push_back(_T3.block(0, 0, 3, 3));
	R.push_back(_T4.block(0, 0, 3, 3));
	return R;
}

std::vector<Eigen::Vector3d> Data::Gett()
{
	std::vector<Eigen::Vector3d> t;
	t.push_back(_T1.block(0, 3, 3, 1));
	t.push_back(_T2.block(0, 3, 3, 1));
	t.push_back(_T3.block(0, 3, 3, 1));
	t.push_back(_T4.block(0, 3, 3, 1));
	return t;
}

std::vector<cv::Mat> Data::GetRGB()
{
	std::vector<cv::Mat> Vcimg_;
	Vcimg_.push_back(_rgb1);
	Vcimg_.push_back(_rgb2);
	Vcimg_.push_back(_rgb3);
	Vcimg_.push_back(_rgb4);
	return Vcimg_;
}

void Data::Clear()
{
	//_depth1 = cv::Mat::zeros(4,2,CV_8UC3);
	_depth1.release();
	_depth2.release();
	_depth3.release();
	_depth4.release();
	_rgb1.release();
	_rgb2.release();
	_rgb3.release();
	_rgb4.release();
}

void Data::ReadFile() {
	std::istringstream iss;
	std::ifstream inf;
	std::string s;
	Eigen::Matrix<double, 32, 4> Mrawdata;
	Mrawdata.setZero();
	inf.open(_FilePath);
	int rows = 0;
	while (getline(inf,s))
	{
		int cols = 0;
		iss.clear();
		iss.str(s);
		double tempdata;
		while (iss >> tempdata) 
		{
			Mrawdata(rows, cols) = tempdata;
			cols++;
		}
		rows++;
	}
	_k1 = Mrawdata.block(0, 0, 3, 3);
	_k2 = Mrawdata.block(3, 0, 3, 3);
	_k3 = Mrawdata.block(6, 0, 3, 3);
	_k4 = Mrawdata.block(9, 0, 3, 3);
	_T1 = Mrawdata.block(12, 0, 4, 4);
	_T2 = Mrawdata.block(16, 0, 4, 4);
	_T3 = Mrawdata.block(20,0, 4, 4);
	_T4 = Mrawdata.block(24, 0, 4, 4);
	p1 = Mrawdata.block(28, 0, 1, 3).transpose();
	p2 = Mrawdata.block(29, 0, 1, 3).transpose();
	p3 = Mrawdata.block(30, 0, 1, 3).transpose();
	p4 = Mrawdata.block(31, 0, 1, 3).transpose();

	
}